#include "typedef.h"
#include "_2misc.h"
#include "astralapi.h"


static struct AstralSpin syslock;
static struct AstralSpin qlock;
static word core_irq_status[CFG_CORE_NUM];
static int ref_cnt[CFG_CORE_NUM];
static int wait_list[CFG_CORE_NUM];
static int wait_rptr;
static int wait_wptr;


#define SYSLOCK_MAGIC 0xa0
void enter_system_critical()
{
    word irq_status = arch_disable_local_irq();

    int core = arch_get_cpuid();
    int core_flag = SYSLOCK_MAGIC | core;

    if (core_flag == syslock.lock)
    {
        ref_cnt[core]++;
        arch_enable_local_irq(irq_status);
        return;
    }

    // let this core enqueue
    while (!arch_compare_and_swap(&qlock, 0x0, core_flag))
    {
    }
    wait_list[wait_wptr] = core_flag;
    wait_wptr = (wait_wptr + 1) % CFG_CORE_NUM;
    while (!arch_compare_and_swap(&qlock, core_flag, 0x0))
    {
    }

    // wait to the head
    while (wait_list[wait_rptr] != core_flag)
    {
        // asm volatile("wfe");
        // arch_enable_local_irq(irq_status);
        // irq_status = arch_disable_local_irq();
    }
    while (syslock.lock != 0)
    {
    }
    syslock.lock = core_flag;
    wait_rptr = (wait_rptr + 1) % CFG_CORE_NUM;

    core_irq_status[core] = irq_status;
    ref_cnt[core]++;
}

void exit_system_critical()
{
    int core = arch_get_cpuid();
    int core_flag = SYSLOCK_MAGIC | core;

    ref_cnt[core]--;
    if (ref_cnt[core] == 0)
    {

        while (!arch_compare_and_swap(&syslock, core_flag, 0x0))
        {
        }

        word status = core_irq_status[core];
        arch_enable_local_irq(status);
    }
    else
    {
        // nothing to do
    }
}
